#include "mypid.h"
#include <math.h>

#define PID_DEFAULT_KP          (1.0f)
#define PID_DEFAULT_KI          (0.0f)
#define PID_DEFAULT_KD		    (0.0f)
#define PID_DEFAULT_ERRIGNORETH (0.02f)

void pid_init(PID_t* pid, float kP, float kI, float kD)
{
	memset(pid, 0, sizeof(PID_t));

	if (kP != 0) {
        pid->kP = kP;
        pid->kI = kI;
        pid->kD = kD;
    }
    else {
        pid->kP = PID_DEFAULT_KP;
        pid->kI = PID_DEFAULT_KI;
        pid->kD = PID_DEFAULT_KD;
    }
    
    pid->errorIgnore_TH = PID_DEFAULT_ERRIGNORETH;
    
    return;
}

void pid_setThreshold(PID_t* pid, float errorICS_TH, float outputMAX_TH, float errorIgnore_TH)
{
    pid->errorICS_TH = errorICS_TH;
    pid->errorIgnore_TH = errorIgnore_TH;
    pid->outputMAX_TH = outputMAX_TH;  
    return;
}

void pid_setICS(PID_t* pid, float errorICS)
{
    pid->errorICS = errorICS;
    
    if (pid->errorICS_TH != 0) {                    // error Integral Calc Sum limiting work.
        if(pid->errorICS > pid->errorICS_TH)
            pid->errorICS = pid->errorICS_TH;
        else if (pid->errorICS < -pid->errorICS_TH)
            pid->errorICS = -pid->errorICS_TH;
    } 
    return;
}

void pid_setSetpoint(PID_t* pid, float setpoint)
{
    pid->setpoint = setpoint;   
    return;
}

void pid_enable(PID_t* pid, int flag)
{
	pid->enable = flag;
    return;
}

float pid_update(PID_t* pid, float present)
{
    if (pid->enable == 0)
        return 0.0f;
    
    pid->present = present;
    
    float err = pid->present-pid->setpoint;
    float abserr = fabsf(err);
    
    if(abserr < pid->errorIgnore_TH)
         err = 0;
    
    pid->errorICS += err;
    
    if (pid->errorICS_TH != 0) {                 // error Integral Calc Sum limiting work.
        if(pid->errorICS > pid->errorICS_TH)
            pid->errorICS = pid->errorICS_TH;
        else if (pid->errorICS < -pid->errorICS_TH)
            pid->errorICS = -pid->errorICS_TH;
    }
    
    float output = pid->kP * err + pid->kI * pid->errorICS + pid->kD * (err - pid->errorLast);
    pid->errorLast = err;
    
    if (pid->outputMAX_TH != 0) {               // output limiting work.
        if(output > pid->outputMAX_TH)
            output = pid->outputMAX_TH;
        else if (output < -pid->outputMAX_TH)
            output = -pid->outputMAX_TH;
    }
    
    return output;
}

